package com.domain.duinocontrolmotor.controller;

import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.util.Log;

import com.domain.duinocontrolmotor.model.ArduinoDevice;
import com.domain.duinocontrolmotor.model.TBlue;
import com.domain.duinocontrolmotor.util.Constants;

public class PanelController extends Controller implements SensorEventListener {

	private static final String TAG = PanelController.class.getSimpleName();

	private ArduinoDevice duino;

	//
	// // Acceloremetro
	// SensorManager sensorManager;
	// Sensor sensor;

	public ArduinoDevice getDuino() {
		return duino;
	}

	public void setDuino(ArduinoDevice duino) {
		this.duino = duino;
	}

	public PanelController(ArduinoDevice duino) {
		super();
		this.duino = duino;
	}

	@Override
	public boolean handleMessage(int what, Object data) {
		Log.i(TAG, "handleMessage: " + what);
		// TODO Auto-generated method stub
		switch (what) {
		case Constants.MESSAGE_INIT_DEVICE:
			initBluetooth((String) data);
			// initAccel();
			break;
		case Constants.MESSAGE_CLOSE_DEVICE:
			closeBluetooth();
			// closeAccel();
			break;
		case Constants.MESSAGE_UPDATE_POSITION:
			updatePosition((SensorEvent) data);
			break;
		case Constants.MESSAGE_SEND_POSITION:
			sendPosition();
			break;
		case Constants.MESSAGE_KEY_EVENT:
		default:

		}
		return false;
	}

	/*** Bluetooth ***/
	void initBluetooth(String addressBluetooth) {
		Log.i(TAG, "Bluetooth initialization... ");
		duino.setSkipped(0);
		TBlue tBlue = new TBlue(addressBluetooth);
		if (tBlue.streaming()) {
			Log.i(TAG, "Bluetooth OK. ");
			duino.setBluetooth(tBlue);
		} else {
			Log.i(TAG, "Error: Bluetooth connection failed. ");
		}
	}

	void closeBluetooth() {
		Log.i(TAG, "Bluetooth closing...");
		if (duino.getBluetooth() != null)
			duino.getBluetooth().close();
	}

	// /*** Accelerometer ***/
	//
	// void initAccel() {
	// Log.i(TAG, "Accelerometer initialization... ");
	//
	// sensorManager = (SensorManager) DuinoControlMotorApp.getContext()
	// .getSystemService(Activity.SENSOR_SERVICE);
	// sensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
	// sensorManager.registerListener(this, sensor,
	// SensorManager.SENSOR_DELAY_NORMAL);
	// }
	//
	// void closeAccel() {
	// Log.i(TAG, "Accelerometer closing... ");
	// sensorManager.unregisterListener(this, sensor);
	// }

	/*** Update POSITION from Sensor **/
	void updatePosition(SensorEvent event) {

		Log.i(TAG, "update position... ");
		duino.setAxisX(event.values[1] / Constants.GRAVITY);
		duino.setAxisY(event.values[2] / Constants.GRAVITY);
		duino.setAxisZ(event.values[0] / Constants.GRAVITY);

		updateServoSteering(duino);
	}

	void updateServoSteering(ArduinoDevice duino) {

		float x = duino.getAxisX();

		if (x < -0.30) {
			duino.setServoSteering((float) -0.50);
		} else if (x > 0.30) {
			duino.setServoSteering((float) 0.50);
		} else {
			duino.setServoSteering((float) 0.0);
		}

	}

	/*** Motor calculations for left and right ***/

	double scale(float x, double old_min, double old_max, double new_min,
			double new_max) {
		double old_range = old_max - old_min;
		double new_range = new_max - new_min;
		double ratio = new_range / old_range;

		return new_min + (x - old_min) * ratio;
	}

	void sendPosition() {
		Log.i(TAG, "send position... ");
		float lr = 0;
		TBlue tblue = duino.getBluetooth();
		if ((duino.getSkipped() > 20)) {

			initBluetooth(duino.getAddressBluetooth());

		}

		if (tblue != null) {
			if (!tblue.streaming()) {
				Log.i(TAG, "0");
				duino.setSkipped(duino.getSkipped() + 1);
				return;
			}

			lr = (int) Math.floor(scale(duino.getServoSteering(), -0.5, 0.5,
					0.0, 9.0));
			if (lr < 0)
				lr = 0;
			Log.i("VALORE", "valore lr:" + lr);
			String s = "";
			s += "S";
			s += (char) lr;
			// s += (char) Math.floor(lr * 5 + 5); // 0 <= l <= 10

			s += "U";

			Log.i(TAG, (String.format("%s Sterzata: %3.0f%%  ", s, lr)));
			tblue.write("?");
			String in = tblue.read();
			Log.i(TAG, "" + in);
			if (in.startsWith("L") && tblue.streaming()) {
				Log.i("fb", "Clear to send, sending... ");
				tblue.write(s);
				duino.setSkipped(0);
			}
		}

	}

	@Override
	public void onAccuracyChanged(Sensor arg0, int arg1) {
		// TODO Auto-generated method stub

	}

	@Override
	public void onSensorChanged(SensorEvent arg0) {
		// TODO Auto-generated method stub
		Log.i(TAG, "sensor Changed... ");
	}
}
